#include "myMath.h"

/* 二阶矩阵求逆 */
static void inv22(const float x[4], float y[4])
{
    float r;
    float t;
    if ((float)fabs(x[1]) > (float)fabs(x[0]))
    {
        r = x[0] / x[1];
        t = 1.0F / (r * x[3] - x[2]);
        y[0] = x[3] / x[1] * t;
        y[1] = -t;
        y[2] = -x[2] / x[1] * t;
        y[3] = r * t;
    }
    else
    {
        r = x[1] / x[0];
        t = 1.0F / (x[3] - r * x[2]);
        y[0] = x[3] / x[0] * t;
        y[1] = -r * t;
        y[2] = -x[2] / x[0] * t;
        y[3] = t;
    }
}

/* 三阶矩阵求逆 */
static void inv33(const float x[9], float y[9])
{
    int p1;
    float b_x[9];
    int p2;
    int p3;
    float absx11;
    float absx21;
    float absx31;
    int itmp;
    for (p1 = 0; p1 < 9; p1++)
    {
        b_x[p1] = x[p1];
    }

    p1 = 0;
    p2 = 3;
    p3 = 6;
    absx11 = (float)fabs(x[0]);
    absx21 = (float)fabs(x[1]);
    absx31 = (float)fabs(x[2]);
    if ((absx21 > absx11) && (absx21 > absx31))
    {
        p1 = 3;
        p2 = 0;
        b_x[0] = x[1];
        b_x[1] = x[0];
        b_x[3] = x[4];
        b_x[4] = x[3];
        b_x[6] = x[7];
        b_x[7] = x[6];
    }
    else
    {
        if (absx31 > absx11)
        {
            p1 = 6;
            p3 = 0;
            b_x[0] = x[2];
            b_x[2] = x[0];
            b_x[3] = x[5];
            b_x[5] = x[3];
            b_x[6] = x[8];
            b_x[8] = x[6];
        }
    }

    absx11 = b_x[1] / b_x[0];
    b_x[1] /= b_x[0];
    absx21 = b_x[2] / b_x[0];
    b_x[2] /= b_x[0];
    b_x[4] -= absx11 * b_x[3];
    b_x[5] -= absx21 * b_x[3];
    b_x[7] -= absx11 * b_x[6];
    b_x[8] -= absx21 * b_x[6];
    if ((float)fabs(b_x[5]) > (float)fabs(b_x[4]))
    {
        itmp = p2;
        p2 = p3;
        p3 = itmp;
        b_x[1] = absx21;
        b_x[2] = absx11;
        absx11 = b_x[4];
        b_x[4] = b_x[5];
        b_x[5] = absx11;
        absx11 = b_x[7];
        b_x[7] = b_x[8];
        b_x[8] = absx11;
    }

    absx11 = b_x[5] / b_x[4];
    b_x[5] /= b_x[4];
    b_x[8] -= absx11 * b_x[7];
    absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8];
    absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4];
    y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0];
    y[p1 + 1] = absx21;
    y[p1 + 2] = absx11;
    absx11 = -b_x[5] / b_x[8];
    absx21 = (1.0F - b_x[7] * absx11) / b_x[4];
    y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
    y[p2 + 1] = absx21;
    y[p2 + 2] = absx11;
    absx11 = 1.0F / b_x[8];
    absx21 = -b_x[7] * absx11 / b_x[4];
    y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
    y[p3 + 1] = absx21;
    y[p3 + 2] = absx11;
}

/* 求二阶矩阵逆, 如果无解dA会很小 */
void invet22(const float A[4], float *dA, float invA[4])
{
    int ix;
    float x[4];
    signed char ipiv[2];
    int iy;
    char isodd;
    int k;
    float temp;
    float b_A[4];
    for (ix = 0; ix < 4; ix++)
    {
        x[ix] = A[ix];
    }

    for (ix = 0; ix < 2; ix++)
    {
        ipiv[ix] = (signed char)(1 + ix);
    }

    ix = 0;
    if ((float)fabs(A[1]) > (float)fabs(A[0]))
    {
        ix = 1;
    }

    if (A[ix] != 0.0F)
    {
        if (ix != 0)
        {
            ipiv[0] = 2;
            ix = 0;
            iy = 1;
            for (k = 0; k < 2; k++)
            {
                temp = x[ix];
                x[ix] = x[iy];
                x[iy] = temp;
                ix += 2;
                iy += 2;
            }
        }

        x[1] /= x[0];
    }

    if (x[2] != 0.0F)
    {
        x[3] += x[1] * -x[2];
    }

    *dA = x[0] * x[3];
    isodd = 0;
    if (ipiv[0] > 1)
    {
        isodd = 1;
    }

    if (isodd)
    {
        *dA = -*dA;
    }

    if (*dA == 0.0F)
    {
        for (ix = 0; ix < 2; ix++)
        {
            for (iy = 0; iy < 2; iy++)
            {
                b_A[ix + (iy << 1)] = 0.0F;
                for (k = 0; k < 2; k++)
                {
                    b_A[ix + (iy << 1)] += A[k + (ix << 1)] * A[k + (iy << 1)];
                }
            }
        }

        inv22(b_A, x);
        for (ix = 0; ix < 2; ix++)
        {
            for (iy = 0; iy < 2; iy++)
            {
                invA[ix + (iy << 1)] = 0.0F;
                for (k = 0; k < 2; k++)
                {
                    invA[ix + (iy << 1)] += x[ix + (k << 1)] * A[iy + (k << 1)];
                }
            }
        }
    }
    else
    {
        inv22(A, invA);
    }
}

/* 求三阶矩阵逆, 如果无解dA会很小 */
void invet33(const float A[9], float *dA, float invA[9])
{
    int i0;
    float x[9];
    int j;
    signed char ipiv[3];
    int c;
    char isodd;
    int jA;
    int k;
    int ix;
    float smax;
    float s;
    float b_A[9];
    int iy;
    int ijA;
    for (i0 = 0; i0 < 9; i0++)
    {
        x[i0] = A[i0];
    }

    for (i0 = 0; i0 < 3; i0++)
    {
        ipiv[i0] = (signed char)(1 + i0);
    }

    for (j = 0; j < 2; j++)
    {
        c = j << 2;
        jA = 0;
        ix = c;
        smax = (float)fabs(x[c]);
        for (k = 2; k <= 3 - j; k++)
        {
            ix++;
            s = (float)fabs(x[ix]);
            if (s > smax)
            {
                jA = k - 1;
                smax = s;
            }
        }

        if (x[c + jA] != 0.0F)
        {
            if (jA != 0)
            {
                ipiv[j] = (signed char)((j + jA) + 1);
                ix = j;
                iy = j + jA;
                for (k = 0; k < 3; k++)
                {
                    smax = x[ix];
                    x[ix] = x[iy];
                    x[iy] = smax;
                    ix += 3;
                    iy += 3;
                }
            }

            i0 = (c - j) + 3;
            for (iy = c + 1; iy < i0; iy++)
            {
                x[iy] /= x[c];
            }
        }

        jA = c;
        iy = c + 3;
        for (k = 1; k <= 2 - j; k++)
        {
            smax = x[iy];
            if (x[iy] != 0.0F)
            {
                ix = c + 1;
                i0 = (jA - j) + 6;
                for (ijA = 4 + jA; ijA < i0; ijA++)
                {
                    x[ijA] += x[ix] * -smax;
                    ix++;
                }
            }

            iy += 3;
            jA += 3;
        }
    }

    *dA = x[0];
    isodd = 0;
    for (k = 0; k < 2; k++)
    {
        *dA *= x[(k + 3 * (k + 1)) + 1];
        if (ipiv[k] > 1 + k)
        {
            isodd = !isodd;
        }
    }

    if (isodd)
    {
        *dA = -*dA;
    }

    if (*dA == 0.0F)
    {
        for (i0 = 0; i0 < 3; i0++)
        {
            for (iy = 0; iy < 3; iy++)
            {
                b_A[i0 + 3 * iy] = 0.0F;
                for (jA = 0; jA < 3; jA++)
                {
                    b_A[i0 + 3 * iy] += A[jA + 3 * i0] * A[jA + 3 * iy];
                }
            }
        }

        inv33(b_A, x);
        for (i0 = 0; i0 < 3; i0++)
        {
            for (iy = 0; iy < 3; iy++)
            {
                invA[i0 + 3 * iy] = 0.0F;
                for (jA = 0; jA < 3; jA++)
                {
                    invA[i0 + 3 * iy] += x[i0 + 3 * jA] * A[iy + 3 * jA];
                }
            }
        }
    }
    else
    {
        inv33(A, invA);
    }
}
